#include "WanderBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "SeekBehaviour.h"
#include "BPhysicsMotionState.h"
#include "Engine.h"
#include "LineDrawer.h"

void WanderBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    float random[32];
    for(int i = 0; i < 32; ++i)
    {
        int r = rand();
        int r2 = rand();
        while(r2 == 0)
            r2 = rand();

        if(r2 % 2 != 0)
            random[i] =  info->GetCircleAdjust() * (float)r / (float)r2;
        else
            random[i] = -info->GetCircleAdjust() * (float)r / (float)r2;
    }
    if(m_init == 0)
    {
        int index[2] = {rand()%32,rand()%32};
        m_target.setValue(random[index[0]], 0, random[index[1]]);
        ++m_init;
    }
    //Randomise our wanderTarget
    int index[2] = {rand()%32,rand()%32};
    btVector3 randomadd = btVector3(random[index[0]], 0, random[index[1]]);
    m_target += randomadd;
    m_target.normalize();
    //normalise and multiply by the radius
    m_target *= info->GetCircleRadius();
    btVector3 trans = m_target;
    //transform into world space
    btMatrix3x3 m;
    btVector3 x = info->GetSourceActor()->GetHeading();
    x.setY(0);
    if(x.length2() < 0.001f)
        x.setValue(0,0,1);
    x.normalize();
    //create the second probe (to the right)
    btVector3 y(0,1,0);
    btVector3 z = y.cross(x);
    z.setY(0);
    z.normalize();
    m[2] = z;//z
    m[1] = y;
    m[0] = x; // x

    trans += btVector3(info->GetCircleDistance(),0,0); 
    btTransform t(m, info->GetSourceActor()->GetPosition());
    trans = t * trans;    
    Engine::Instance()->GetLineDrawer()->drawLine(info->GetSourceActor()->GetPosition(), trans, btVector3(1,1,1));
    //add distance to x component


    //Seek to the new location 
    BehaviourI* seek  = new SeekBehaviour();
    info->SetTargetActor(NULL);
    info->SetTargetPosition(&trans);
    seek->CalcMotion(motion, info, weighting);
    delete seek;
}